/*
 * data.c
 *
 *  Created on: Oct 5, 2025
 *      Author: lianz
 */

#include "stm32f1xx_hal.h"
#include "pid.h"
#include "usart.h"


#define LINE	1
#define COLUMN	39

uint32_t sys_tick = 0;
uint8_t time_buf[2][LINE][COLUMN];
//uint8_t roll_buf[2][LINE][7];
//uint8_t lencoder_buf[2][LINE][5];
//uint8_t rencoder_buf[2][LINE][5];
uint8_t data[2][LINE][COLUMN];

uint8_t data_index = 0;

// 记录一次传感器数据
void Note_Data(void)
{
	static uint16_t i = 0;
	// 一个缓存区满了立即发送到串口，切换缓存区
	if (i == LINE)
	{
		i = 0;
		data_index = !data_index;
		// dma发送串口
		HAL_UART_Transmit_DMA(&huart3, data[!data_index][0], sizeof(data[!data_index][0]));
	}
	// 填充数据
	// 组头
	uint8_t glen = COLUMN;
	data[data_index][i][0] = 0xFF;
	data[data_index][i][1] = 0xFE;
	data[data_index][i][2] = 0x00;
	data[data_index][i][3] = 0xFE;
	data[data_index][i][4] = glen;
	// 时间-四字节
	uint8_t ti = 5;
	uint8_t tlen = 7;	//帧长度（包含帧头，即整个帧）
	int32_t time = sys_tick;	// 时间数据快照
	data[data_index][i][ti + 0] = 0xAA;
	data[data_index][i][ti + 1] = 0x55;
	data[data_index][i][ti + 2] = tlen;
	data[data_index][i][ti + 3] = time >> 24;
	data[data_index][i][ti + 4] = time >> 16;
	data[data_index][i][ti + 5] = time >> 8;
	data[data_index][i][ti + 6] = time;
	// 翻滚角度值-四字节
	uint8_t ri = 12;
	uint8_t rlen = 7;
	int32_t roll_d = roll * 100;
	data[data_index][i][ri + 0] = 0xAA;
	data[data_index][i][ri + 1] = 0x55;
	data[data_index][i][ri + 2] = rlen;
	data[data_index][i][ri + 3] = roll_d >> 24;
	data[data_index][i][ri + 4] = roll_d >> 16;
	data[data_index][i][ri + 5] = roll_d >> 8;
	data[data_index][i][ri + 6] = roll_d;
	// 左编码器
	uint8_t lei = 19;
	uint8_t lelen = 5;
	int16_t lencoder_data = med_angle;
	data[data_index][i][lei + 0] = 0xAA;
	data[data_index][i][lei + 1] = 0x55;
	data[data_index][i][lei + 2] = lelen;
	data[data_index][i][lei + 3] = lencoder_data >> 8;
	data[data_index][i][lei + 4] = lencoder_data;
	// 右编码器
	uint8_t rei = 24;
	uint8_t relen = 5;
	int16_t rencoder_data = Rencoder;
	data[data_index][i][rei + 0] = 0xAA;
	data[data_index][i][rei + 1] = 0x55;
	data[data_index][i][rei + 2] = relen;
	data[data_index][i][rei + 3] = rencoder_data >> 8;
	data[data_index][i][rei + 4] = rencoder_data;
	// 左驱动器
	uint8_t lmi = 29;
	uint8_t lmlen = 5;
	int16_t lmoto_data = moto1;
	data[data_index][i][lmi + 0] = 0xAA;
	data[data_index][i][lmi + 1] = 0x55;
	data[data_index][i][lmi + 2] = lmlen;
	data[data_index][i][lmi + 3] = lmoto_data >> 8;
	data[data_index][i][lmi + 4] = lmoto_data;
	// 补偿角度
	uint8_t rmi = 34;
	uint8_t rmlen = 5;
	int16_t rmoto_data = moto2;
	data[data_index][i][rmi + 0] = 0xAA;
	data[data_index][i][rmi + 1] = 0x55;
	data[data_index][i][rmi + 2] = rmlen;
	data[data_index][i][rmi + 3] = rmoto_data >> 8;
	data[data_index][i][rmi + 4] = rmoto_data;
	//data[data_index][i][1] = roll;
	//data[data_index][i][2] = Lencoder;
	//data[data_index][i][3] = Rencoder;
	i++;
}

